#include "./../Headers/vector_3d.h"

namespace Mathematics {
	namespace Geometry {

		bool Vector3D::create(){
			coord = (double*) malloc( spt_dim * sizeof( double ) );
			if( coord == NULL )
				return false;
			for( usint i = 0; i < spt_dim; ++i )
				coord[i] = 0.0;
			return true;
		};
		void Vector3D::copy( const Vector3D &original ){
			for( usint i = 0; i < spt_dim; ++i )
				coord[i] = original.coord[i];
		};
		void Vector3D::free(){
			delete coord;
		}
		
		void Vector3D::verifyMod(){
			if( max_mod <= 0.0 ) return;	// nothing to do.
			
			double cur_mod = mod();
			if( cur_mod > max_mod ){
				(*this) *= max_mod / cur_mod;
			}
		};
		
		Vector3D::Vector3D( const double &x, const double &y, const double &z, const double &max ):
			coord(),max_mod(max){
		#ifdef DEBUG_GEOM_VECTOR
			debug_small( "Constructor", "Vector3D", (int) &( *this ), sizeof( *this ) );
		#endif
			if( !create() ){
				cout << "Couldn't create coordinates array!" << endl;
				exit(-1);
			}
			coord[0] = x;	coord[1] = y;	coord[2] = z;
			verifyMod();
		};
		Vector3D::Vector3D(const Vector3D &original):
			max_mod(-1.0){
		#ifdef DEBUG_GEOM_VECTOR
			debug_small( "Constructor (copy)", "Vector3D", (int) &( *this ), sizeof( *this ) );
		#endif
			if( !create() ){
				cout << "Couldn't create coordinates array!" << endl;
				exit(-1);
			}
			copy( original );
		};
		Vector3D::~Vector3D(){
		#ifdef DEBUG_GEOM_VECTOR
			debug_small( "Destructor", "Vector3D", (int) &( *this ), sizeof( *this ) );
		#endif
			free();
		};
		
		void Vector3D::setMod( const double &value ) {
			double cur_mod = mod();
			if( cur_mod > 0.0 ){
				(*this) *= value / cur_mod;
			}
			verifyMod();
		};
		double Vector3D::mod() const {
			return sqrt( (*this) * (*this) );
		};
		Vector3D Vector3D::versor() const {
			double module = mod();
			if( module == 0.0 ) return Vector3D();
			return (*this) / mod();
		};
		
		Vector3D Vector3D::projectedOn( const Vector3D &vet ) const {
			// return vet.versor() * (vet.versor() * (*this));
			Vector3D result( vet );
			result.setMod( 1.0 );
			result *= result * (*this);
			return result;
		};
		Vector3D Vector3D::reflectedOn( const Vector3D &vet ) const {
			// return (this->projectedOn(v) * 2) - (*this);
			Vector3D result( vet );
			result.setMod( 1.0 );
			result *= ( result * (*this) ) * 2;
			result -= (*this);
			return result;
		};
		//Vector3D Vector3D::zRotated( const double &degree ) const {
		//	using namespace Mathematics::Radian;
		//	double cosseno = RCos(degree), seno = RSin(degree);
		//	return Vector3D( (coord[X] * cosseno) - (coord[Y] * seno),
		//			 (coord[X] * seno) + (coord[Y] * cosseno), coord[Z] );
		//};
		
		Vector3D& Vector3D::operator = (const Vector3D &vet){
			copy(vet);
			verifyMod();
			return (*this);
		};
		Vector3D Vector3D::operator + (const Vector3D &vet) const {
			Vector3D result;
			for( usint i = 0; i < spt_dim; ++i ){
				result.coord[i] = coord[i] + vet.coord[i];
			}
			return result;
		};
		Vector3D& Vector3D::operator += (const Vector3D &vet) {
			for( usint i = 0; i < spt_dim; ++i ){
				coord[i] += vet.coord[i];
			}
			verifyMod();
			return (*this);
		};
		Vector3D Vector3D::operator - (const Vector3D &vet) const {
			Vector3D result;
			for( usint i = 0; i < spt_dim; ++i ){
				result.coord[i] = coord[i] - vet.coord[i];
			}
			return result;
		};
		Vector3D& Vector3D::operator -= (const Vector3D &vet) {
			for( usint i = 0; i < spt_dim; ++i ){
				coord[i] -= vet.coord[i];
			}
			verifyMod();
			return (*this);
		};
		Vector3D Vector3D::operator % (const Vector3D &vet) const {
			Vector3D result( 0.0, 0.0, 0.0 );
			for( usint i = 0; i < spt_dim; ++i ){
				result.coord[i] +=
					coord[ (i+1)%spt_dim ] * vet.coord[ (i+2)%spt_dim ]
					- coord[ (spt_dim+i-1)%spt_dim ] * vet.coord[ (spt_dim+i-2)%spt_dim ];
			}
			return result;
		};
		Vector3D& Vector3D::operator %= (const Vector3D &vet) {
			Vector3D result( 0.0, 0.0, 0.0 );
			for( usint i = 0; i < spt_dim; ++i ){
				result.coord[i] +=
					coord[ (i+1)%spt_dim ] * vet.coord[ (i+2)%spt_dim ]
					- coord[ (spt_dim+i-1)%spt_dim ] * vet.coord[ (spt_dim+i-2)%spt_dim ];
			}
			for( usint i = 0; i < spt_dim; ++i ){
				coord[i] = result.coord[i];
			}
			verifyMod();
			return (*this);
		};
		double Vector3D::operator * (const Vector3D &vet) const {
			double result = 0.0;
			for( usint i = 0; i < spt_dim; ++i ){
				result += coord[i] * vet.coord[i];
			}
			return result;
		};
		Vector3D Vector3D::operator * (const double &scalar) const {
			Vector3D result;
			for( usint i = 0; i < spt_dim; ++i ){
				result.coord[i] = coord[i] * scalar;
			}
			return result;
		};
		Vector3D& Vector3D::operator *= (const double &scalar) {
			for( usint i = 0; i < spt_dim; ++i ){
				coord[i] *= scalar;
			}
			verifyMod();
			return (*this);
		};
		Vector3D Vector3D::operator / (const double &scalar) const {
			if( scalar == 0.0 ){
				throw DivisionByZeroException<Vector3D>( "Dividing by zero", (*this) );
			}
			Vector3D result;
			for( usint i = 0; i < spt_dim; ++i ){
				result.coord[i] = coord[i] / scalar;
			}
			return result;
		};
		Vector3D& Vector3D::operator /= (const double &scalar){
			if( scalar == 0.0 ){
				throw DivisionByZeroException<Vector3D>( "Dividing by zero", (*this) );
			}
			for( usint i = 0; i < spt_dim; ++i ){
				coord[i] /= scalar;
			}
			verifyMod();
			return (*this);
		};
		bool Vector3D::operator == (const Vector3D &vet) const {
			for( usint i = 0; i < spt_dim; ++i ) if( coord[i] != vet.coord[i] ) return false;
			return true;
		};
		double& Vector3D::operator [] (const usint &index) const {
			if( index >= spt_dim ){
				throw OutOfBoundsException("Acesso Invalido.",spt_dim,index);
			}
			return coord[index];
		};
		double& Vector3D::operator [] (const CartesianCoordinate &index) const {
			return coord[(int)index];
		};
		Vector3D::operator Matrix::Matrix1D () const {
			Matrix::Matrix1D result(spt_dim+1);
			for( usint i = 0; i < spt_dim; ++i )
				result[i] = coord[i];
			result[spt_dim] = 1.0;
			return result;
		};
		Vector3D operator * ( const Matrix::Matrix2D &mat, const Vector3D &vet ) {
			using namespace Matrix;
			Matrix2D temp( mat * (Matrix2D)vet );
			Vector3D result;
			for( usint i = 0; i < Vector3D::spt_dim; ++i )
				result[i] = temp[i][0];
			if( temp[Vector3D::spt_dim][0] != 0.0 )
				result /= temp[Vector3D::spt_dim][0];
			return result;
		};
		double* Vector3D::toPointer() const {
			double *result = NULL;
			result = (double*) malloc( spt_dim * sizeof(double) );
			if( result == NULL ){
				cout << "Couldn't allocate memory!" << endl;
				exit(-1);
			}
			for( usint i = 0; i < spt_dim; ++i )
				result[i] = coord[i];
			return result;
		};
		float* Vector3D::toPointerf() const {
			float *result = NULL;
			result = (float*) malloc( spt_dim * sizeof(float) );
			if( result == NULL ){
				cout << "Couldn't allocate memory!" << endl;
				exit(-1);
			}
			for( usint i = 0; i < spt_dim; ++i )
				result[i] = (float) coord[i];
			return result;
		};
		string Vector3D::toS() const {
			string result = "( ";
			for( usint i = 0; i < spt_dim; ++i ){
				result += dtos( coord[i] ) + ( (usint)(i+1) < spt_dim ? ", " : " " );
			}
			result += ")";
			return result;
		};
		bool Vector3D::filePrint( FILE *parq ) const {
			return fprintf( parq, "%s", toS().c_str() ) == 1;
		};

		//void Vector3D::test(){
		//	cout << endl << "---Test:Vector3D---" << endl;
		//	
		//	using namespace OrthonormalBase;
		//	cout << "OrthonormalBase:" << endl;
		//	cout << "uX:" << OrthonormalBase::uX << endl;
		//	cout << "uY:" << OrthonormalBase::uY << endl;
		//	cout << "uZ:" << OrthonormalBase::uZ << endl;
		//	
		//	Vector3D vet1(1.0,2.5,-1.0);
		//	Vector3D vet2(0.0,2.0,1.2);
		//	
		//	cout << "Vet1 : " << vet1 << endl;
		//	cout << "Vet1.mod() : " << vet1.mod() << endl;
		//	cout << "Vet1.versor() : " << vet1.versor() << endl;
		//	cout << "Vet2 : " << vet2 << endl;
		//	
		//	cout << "Vet1.getCoord(0) : " << vet1.getCoord(0) << endl;
		//	cout << "Vet1[0] : " << vet1[0] << endl;
		//	cout << "Vet1[X] : " << vet1[X] << endl;
		//	cout << "Vet1[Y] : " << vet1[Y] << endl;
		//	cout << "Vet1[Z] : " << vet1[Z] << endl;
		//	
		//	cout << "Vet1 + Vet2 : " << vet1 + vet2 << endl;
		//	cout << "Vet1 - Vet2 : " << vet1 - vet2 << endl;
		//	cout << "Vet1 += Vet2 : " << (vet1 += vet2) << endl;
		//	cout << "Vet1 : " << vet1 << endl;
		//	cout << "Vet1 -= Vet2 : " << (vet1 -= vet2) << endl;
		//	cout << "Vet1 : " << vet1 << endl;
		//	
		//	cout << "Vet1 * 3.0 : " << vet1 * 3.0 << endl;
		//	cout << "Vet1 : " << vet1 << endl;
		//	cout << "Vet1 *= 5.0 : " << (vet1 *= 5.0) << endl;
		//	cout << "Vet1 : " << vet1 << endl;
		//	cout << "Vet1 /= 2.0 : " << (vet1 /= 2.0) << endl;
		//	cout << "Vet1 : " << vet1 << endl;
		//	
		//	cout << "Vet1 * Vet2 : " << vet1 * vet2 << endl;
		//	cout << "Vet1 % Vet2 : " << vet1 % vet2 << endl;
		//	cout << "Vet1 : " << vet1 << endl;
		//	cout << "Vet1 %= Vet2 : " << (vet1 %= vet2) << endl;
		//	cout << "Vet1 : " << vet1 << endl;
		//	
		//	cout << "Vet1.isColinear( Vet2 ) : " << vet1.isColinear(vet2) << endl;
		//	cout << "Vet1.isColinear( Vet1 ) : " << vet1.isColinear(vet1) << endl;
		//	cout << "Vet1.isColinear( Vet1/2.0 ) : " << vet1.isColinear(vet1/2.0) << endl;
		//	
		//	cout << "Vet1.versor() : " << vet1.versor() << endl;
		//	cout << "Vet1.reflectedOn( uX ) : " << vet1.reflectedOn(uX) << endl;
		//	cout << "Vet1.reflectedOn( Vet2 ) : " << vet1.reflectedOn(vet2) << endl;
		//	
		//	cout << "Vet1.projectedOn( uX ) : " << vet1.projectedOn(uX) << endl;
		//	cout << "Vet1.projectedOn( Vet2 ) : " << vet1.projectedOn(vet2) << endl;
		//	
		//	cout << "uX.reflectedOn( uZ ) : " << uX.reflectedOn(uZ) << endl;
		//	cout << "(2.5*uX).reflectedOn( uY + uZ ) : " << (2.5*uX).reflectedOn( uY + uZ ) << endl;
		//	
		//	using namespace Matrix;
		//	cout << "Vet1 : " << (Matrix1D) vet1 << endl;
		//	cout << "Vet1 : " << (Matrix2D) vet1 << endl;
		//	
		//	Matrix2D mat(4,4);
		//	mat,
		//		1.0, 0.0, 0.0, 0.0, nl,
		//		0.0, 2.0, 0.0, 0.0, nl,
		//		0.0, 0.0, 1.0, 0.0, nl,
		//		0.0, 0.0, 0.0, 2.0;
		//	cout << "Mat : " << mat << endl;
		//	cout << "Mat * (Matrix2D)Vet1 : " << mat * (Matrix2D) vet1 << endl;
		//	cout << "Mat * Vet1 : " << mat * vet1 << endl;
		//	
		//	cout << "Scalar product:" << endl;
		//	cout << "uX * uY : " << uX * uY << endl;
		//	cout << "Vectorial product:" << endl;
		//	cout << "uX % uY : " << uX % uY << endl;
		//	
		//	cout << endl << "---TestEnd---" << endl;
		//};
		
		namespace OrthonormalBase {
			const Vector3D uX(1.0,0.0,0.0,1.0);
			const Vector3D uY(0.0,1.0,0.0,1.0);
			const Vector3D uZ(0.0,0.0,1.0,1.0);
		};
		
	};
};
